#include "cloudpointprocess.h"

CloudPointProcess::CloudPointProcess(QObject *parent)
    : QThread{parent}
{
    quit = false;
    pause_status = false;
    idx_num = -50;
}

void CloudPointProcess::run()
{
    qDebug()<<"看我多帅呦";
    forever
    {
        QThread::usleep(33000);
        if(quit) break;

        initializePointCloud();

    }
    qDebug()<<"帅逼退下";
    quit = false;
}

void CloudPointProcess::stop()
{
    quit = true;
}

void CloudPointProcess::initializePointCloud()
{
    // 创建一个新的点云对象并分配内存
    Cloud::Ptr cloud(new Cloud());
//    // 设置点云大小为1000个点（单层）
//    cloud->width = 1280;
//    cloud->height = 1;
//    cloud->is_dense = true; // 假设点云是稠密的，没有NaN值
//    // 分配足够的空间存储1000个点的数据
//    cloud->points.resize(cloud->width * cloud->height);
//    // 填充1000个在同一平面的点数据，这里假设平面为z=0
//    for (size_t i = 0; i < cloud->points.size(); ++i) {
//        float x = rand() % 500 - 250; // 在-250到250范围内随机生成x坐标
//        float y = rand() % 500 - 250; // 在-250到250范围内随机生成y坐标
//        float z = rand() % 500 - 250; // 在-250到250范围内随机生成y坐标
//        cloud->points[i].x = x;
//        cloud->points[i].y = y;
//        cloud->points[i].z = z; // 所有点的z坐标均为0，确保它们在同一平面
//    }
    // 创建并填充心形点云
    int resolution = 1280; // 控制点云精细度
//    *cloud = createHeartShape(resolution);
    *cloud = creatHalfRingShape(resolution);
    emit send_cloud_point(cloud);
}


Cloud CloudPointProcess::createHeartShape(int resolution) {
    Cloud heart;
    const float PI = 3.14159265358979323846f;
    for (int i = 0; i < resolution; ++i) {
        float t = static_cast<float>(i) * 2 * PI / static_cast<float>(resolution);
        float r = 16 * pow(sin(t), 3);
        float x = r * cos(t);
        float y = r * sin(t);
        float z = 13 * cos(t) + 5;
        heart.push_back(PointT(x, y, z));
    }
    idx_num++;
    return heart;
}

Cloud CloudPointProcess::creatHalfRingShape(int resolution)
{
    Cloud cloud;
    // 半圆参数：中心、半径、点的数量（即“高度”）
    double centerX = 0.0;
    double radius = 100.0;
    int numPointsAlongArc = resolution;

    // 计算每个点的角度间隔
    double stepAngle = M_PI / static_cast<double>(numPointsAlongArc - 1); // 仅创建0-180度部分

    for (int i = 0; i < numPointsAlongArc; ++i)
    {
        double angle = static_cast<double>(i) * stepAngle;
        double x = centerX + radius * cos(angle);
        double z = radius * sin(angle);

        pcl::PointXYZ point(x, 0, z); // Y坐标始终为0
        cloud.push_back(point);
    }

    // 设置点云的大小信息
    cloud.width = numPointsAlongArc;
    cloud.height = 1;
    cloud.is_dense = true; // 如果所有点都在半圆上则可以设置为true


    return cloud;
}
